// TP2Metodos.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"// TPMetodos.cpp : Defines the entry point for the console application.
#include <iostream>
#include <sstream>
#include <vector>
#include <stdio.h>
#include <math.h>
#include <string>
#include "Matriz.h"
#include "InputFile.h"
#include "herramientas.h"
#include <cstdlib>
#include <time.h>

using namespace std;

const double G = 1.488180711e-34; //AU kg dia
const bool escribirEncabezado = false;
const int cantIteraciones = 3; //Iteraciones para el metodo 3

vector<double> Fij(double m1, double m2, vector<double>o1, vector<double>o2){

    double normacubo;

    o1[0] = o1[0] - o2[0];
    o1[1] = o1[1] - o2[1];
    o1[2] = o1[2] - o2[2];

    double moduloObj1 = modulo(o1);
	normacubo = moduloObj1*moduloObj1*moduloObj1;

    double x,y,z;

    x = -1*G*m1*m2*o1[0]/normacubo;
    y = -1*G*m1*m2*o1[1]/normacubo;
    z = -1*G*m1*m2*o1[2]/normacubo;

    vector<double> res;

    res.push_back(x);
    res.push_back(y);
    res.push_back(z);

    return res;
}

vector<double> ai(Matriz* Y, const vector<double>& m, int i){
    int n = m.size();
    int j;

    vector<double> res;
    vector<double> temp;

    double x = 0;
    double y = 0;
    double z = 0;

    for (j=1; j<=n; j++){
        if (j!=i){
            temp = Fij(m[i-1],m[j-1],Y->objetoPos(i),Y->objetoPos(j));
            x+=temp[0];
            y+=temp[1];
            z+=temp[2];
        }
    }

    res.push_back(x/m[i-1]);
    res.push_back(y/m[i-1]);
    res.push_back(z/m[i-1]);

    return res;
}

Matriz* derivada(Matriz* Y, const vector<double>& masas){

    unsigned int n = masas.size()*6;

    Matriz* res = new Matriz(n,1);
    vector<double> acelTemp;

    unsigned int i;

    for (i=1;i<=n/2;i++){
        res->setValor(i+(n/2),1,Y->getValor(i,1));
    }

    for (i=1;i<=masas.size();i++){
        acelTemp = ai(Y,masas,i);
        res->setValor((i-1)*3+1,1,acelTemp[0]);
        res->setValor((i-1)*3+2,1,acelTemp[1]);
        res->setValor((i-1)*3+3,1,acelTemp[2]);
    }

    return res;

}

Matriz* DerivadaDeFijRespectoAXj(Matriz* Y, const vector<double>& masas, int i, int j){

    Matriz* S = new Matriz(3,3);
    Matriz* I = new Matriz(3,3);

    I->llenarConIdentidad();

    double normad;
	vector<double> d;    
    vector<double> xi = Y->objetoPos(i);
    vector<double> xj = Y->objetoPos(j);

    d.push_back(xi[0]-xj[0]);
    d.push_back(xi[1]-xj[1]);
    d.push_back(xi[2]-xj[2]);

	S->DPorDTraspuesta(d);
    normad = modulo(d);

    I->multiplicarEscalar(1/(normad*normad*normad));
    S->multiplicarEscalar(-3/(normad*normad*normad*normad*normad));
    I->suma(S);
    
    I->multiplicarEscalar(G*masas[i-1]*masas[j-1]);

	delete(S);
	
    return I;
}


Matriz* Df(const vector<double>& masas, Matriz* Y){

	unsigned int i,j,k,n;

	n = masas.size()*3;
	
    Matriz* A11 = new Matriz(n,n);
    Matriz* A22 = new Matriz(n,n);
    Matriz* A21 = new Matriz(n,n);
    Matriz* A12 = new Matriz(n,n);

    A21->llenarConIdentidad();

    Matriz* temp;
	Matriz* acumulador;

    for (i=1; i<=masas.size(); i++){
        for (j=1; j<=masas.size(); j++){
            if (j==i){

                acumulador = new Matriz(3,3);

                for (k=1; k<=masas.size(); k++){
                    if (k!=i){
                        temp = DerivadaDeFijRespectoAXj(Y,masas,i,k);
						temp->multiplicarEscalar(-1);						
                        acumulador->suma(temp);
						delete(temp);
                    }
                }

                acumulador->multiplicarEscalar(1/masas[i-1]);
                A12->ubicarMatriz(acumulador,(i-1)*3+1,(j-1)*3+1);
				delete(acumulador);

            }else{
                temp = DerivadaDeFijRespectoAXj(Y,masas,i,j);
                temp->multiplicarEscalar(1/masas[i-1]);
                A12->ubicarMatriz(temp,(i-1)*3+1,(j-1)*3+1);
				delete(temp);
            }
        }
    }

    Matriz* DfRes = new Matriz(A11,A12,A21,A22);

	delete(A11);
	delete(A12);
	delete(A21);
	delete(A22);

    return DfRes;	
}

void metodo1(const vector<string>& opciones, const vector<string>& nombres, const vector<double>& masas, Matriz* Y, double dt, int n, int k){	
	Matriz* Y1;
	Matriz* temp;
	vector<double> Cuerpo;
	int i,p;
	
	abrirArchivos(opciones);

	Y1 = new Matriz(Y);
	
	for (p=1; p<=n; p++){
		if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
			Cuerpo = Y1->objetoPos(p);
			
			escribirEnArchivo(nombres[p-1],Cuerpo,escribirEncabezado,k);
		}
	}

	for (i=1; i<=k; i++){
		temp = derivada(Y1,masas);
		temp->multiplicarEscalar(dt);
		Y1->suma(temp);
		delete(temp);

		for (p=1; p<=n; p++){
			if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
				Cuerpo = Y1->objetoPos(p);
				
				escribirEnArchivo(nombres[p-1],Cuerpo,false,k);
			}
		}
	}

	cerrarArchivos(opciones);

	delete(Y1);
	
}


void metodo2(const vector<string>& opciones, const vector<string>& nombres, const vector<double>& masas, Matriz* Y, double dt, int n, int k){
	Matriz* Y2;
	vector<double> Cuerpo;
	Matriz* DF;
	Matriz* A;
	Matriz* b;
	int i,p;

	abrirArchivos(opciones);

	Y2 = new Matriz(Y);

	for (p=1; p<=n; p++){
		if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
			Cuerpo = Y2->objetoPos(p);
			escribirEnArchivo(nombres[p-1],Cuerpo,escribirEncabezado,k);
		}
	}

	for (i=1; i<=k; i++){
	
		DF = Df(masas, Y2);
		DF->multiplicarEscalar(-dt);

		A = new Matriz(DF->cantFilas(), DF->cantColumnas());
		A->llenarConIdentidad();
		A->suma(DF);
		
		DF->multiplicar(Y2);

		b = derivada(Y2,masas);
		b->multiplicarEscalar(dt);
		b->suma(DF);
		b->suma(Y2);

		delete(Y2);
		Y2 = A->resolverSistema(b);

		delete(DF);
		delete(A);
		delete(b);

		for (p=1; p<=n; p++){
			if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
				Cuerpo = Y2->objetoPos(p);
				escribirEnArchivo(nombres[p-1],Cuerpo,false,k);
			}
		}		
	}

	cerrarArchivos(opciones);

	delete(Y2);
}

void metodo3(const vector<string>& opciones, const vector<string>& nombres, const vector<double>& masas, Matriz* Y, double dt, int n, int k){

	Matriz* Y3;
	Matriz* wk;
	Matriz* wkMasUno;
	Matriz* A;
	Matriz* DF;
	Matriz* b;
	vector<double> Cuerpo;
	int i,p,j;

	abrirArchivos(opciones);

	wk = new Matriz(Y);
	Y3 = new Matriz(Y);

	for (p=1; p<=n; p++){
		if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
			Cuerpo = wk->objetoPos(p);
			escribirEnArchivo(nombres[p-1],Cuerpo,escribirEncabezado,k);
		}
	}
	int MaxIter=0;
	for (i=1; i<=k; i++){
		j=1;
		double dif =1;
		
		while (j <= cantIteraciones && dif >= 0.001){
			DF = Df(masas, wk);
			DF->multiplicarEscalar(-dt);

			A = new Matriz(DF->cantFilas(), DF->cantColumnas());
			A->llenarConIdentidad();
			A->suma(DF);
			
			DF->multiplicar(wk);

			b = derivada(wk,masas);
			b->multiplicarEscalar(dt);
			b->suma(DF);
			b->suma(Y3);

			wkMasUno = A->resolverSistema(b);

			delete(DF);
			delete(A);
			delete(b);
			dif=wkMasUno->comparar(wk);
			//cout << "dif iteracion "<< j << " " << dif << endl;
			
			delete(wk);
			wk = new Matriz(wkMasUno);
			
			delete(wkMasUno);
			j++;
		}
		//cout << "cant interaciones " << j-1 <<endl;
		
		if ((j-1)>MaxIter){MaxIter=j-1; }
			
			
		Y3 = new Matriz(wk);

		for (p=1; p<=n; p++){
			if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
				Cuerpo = Y3->objetoPos(p);
				escribirEnArchivo(nombres[p-1],Cuerpo,false,k);
			}
		}
	}
	//cout << "cant maxima interaciones " << MaxIter <<endl;
	cerrarArchivos(opciones);
	delete(Y3);
	
}
void metodo4(const vector<string>& opciones, const vector<string>& nombres, const vector<double>& masas, Matriz* Y, double dt, int n, int k){	
	
	Matriz* DF;
	Matriz* A;
	Matriz* b;
	Matriz* Y1;

	Matriz* temp;
	vector<double> Cuerpo;
	int i,p;

	abrirArchivos(opciones);

	Y1 = new Matriz(Y);
	
	for (p=1; p<=n; p++){
		if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
			Cuerpo = Y1->objetoPos(p);
			escribirEnArchivo(nombres[p-1],Cuerpo,escribirEncabezado,k);
		}
	}

	for (i=1; i<=k; i++){
		//Metodo 1
		temp = derivada(Y1,masas);
		temp->multiplicarEscalar(dt);
		temp->suma(Y1);		

		//Metodo 2
		DF = Df(masas, Y1);
		DF->multiplicarEscalar(-dt);

		A = new Matriz(DF->cantFilas(), DF->cantColumnas());
		A->llenarConIdentidad();
		A->suma(DF);
		
		DF->multiplicar(Y1);

		b = derivada(Y1,masas);
		b->multiplicarEscalar(dt);
		b->suma(DF);
		b->suma(Y1);
	
		Y1 = A->resolverSistema(b);

		delete(DF);
		delete(A);
		delete(b);

		//promedio metodo1 y metodo2
		Y1->suma(temp);
		Y1->multiplicarEscalar(0.5);

		delete(temp);

		for (p=1; p<=n; p++){
			if (contiene(opciones,nombres[p-1]) || contiene(opciones,"Todos")){
				Cuerpo = Y1->objetoPos(p);
				escribirEnArchivo(nombres[p-1],Cuerpo,false,k);
			}
		}
	}

	cerrarArchivos(opciones);

	delete(Y1);
	
}

int main(int argc, char *argv[])
{
    int i;

	if (argc >= 3 ){

		srand((unsigned)time(0));

		vector<string> opciones;
	    
		opciones.resize(argc-2);

		for (i=1; i<=argc-2; i++){
			opciones[i-1] = argv[i+1];
		}

		/*opciones.push_back("-m4");
		opciones.push_back("Todos");
		opciones.push_back("Luna");
		opciones.push_back("Tierra");
		opciones.push_back("Sol");*/
		
		InputFile* input = new InputFile();
		input->openFile(argv[1]);
		//input->openFile("input.txt");
		vector<string> datos = input->getFileContent();

		double dt = str2double(datos[0]);
		int k = str2Int(datos[1]);
		int n = str2Int(datos[2]);
		string nombre;
		double masa;
		double vx;
		double vy;
		double vz;
		double x;
		double y;
		double z;

		Matriz* Y = new Matriz(n*6,1);
		vector<double> masas;
		vector<string> nombres;
		
		int cont;
		for (cont = 0; cont < n; cont++){

			nombre = datos[cont*8+3];
			masa = str2double(datos[cont*8+4]);
			vx = str2double(datos[cont*8+5]);
			vy = str2double(datos[cont*8+6]);
			vz = str2double(datos[cont*8+7]);
			x = str2double(datos[cont*8+8]);
			y = str2double(datos[cont*8+9]);
			z = str2double(datos[cont*8+10]);

			nombres.push_back(nombre);
			masas.push_back(masa);

			Y->setValor(3*cont+1,1,vx);
			Y->setValor(3*cont+2,1,vy);
			Y->setValor(3*cont+3,1,vz);
			Y->setValor(3*cont+1+(3*n),1,x);
			Y->setValor(3*cont+2+(3*n),1,y);
			Y->setValor(3*cont+3+(3*n),1,z);
		}

		if (contiene(opciones,"-m1") || contiene(opciones,"-M1")){
			metodo1(opciones,nombres,masas,Y,dt,n,k);
		}else{

			if (contiene(opciones,"-m2") || contiene(opciones,"-M2")){
				metodo2(opciones,nombres,masas,Y,dt,n,k);
			}else{
				if (contiene(opciones,"-m3") || contiene(opciones,"-M3")){
					metodo3(opciones,nombres,masas,Y,dt,n,k);
				}else{
					if (contiene(opciones,"-m4") || contiene(opciones,"-M4")){
						metodo4(opciones,nombres,masas,Y,dt,n,k);
					}
				}
					
			}
		}
	}else{
		cout << "Faltan parametros, una posible forma de uso es: TP2metodos input.txt -m1 -Todos" << endl;
	}

	return 0;
}
